/*
 * Copyright (c) 2024 Black Sesame Technologies
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <cstdlib>
#include "dal_rticonnextdds/converter/QosConverter.hpp"
#include "dal_rticonnextdds/utils/StringUtils.hpp"
#include "dal_rticonnextdds/utils/log.hpp"
#include "dal/config_loader.h"

QosConverter* QosConverter::singleton_ = nullptr;

QosConverter::QosConverter() {}

QosConverter* QosConverter::GetInstance()
{
    if (singleton_ == nullptr) {
        singleton_ = new QosConverter();
    }
    return singleton_;
}

dal_ret_t QosConverter::SetUnicastToRtiddsParticipantQos(dal_domain_participant_qos_t* d_qos,
                                                         DDS_DomainParticipantQos* r_qos)
{
    if (d_qos == nullptr) {
        LOG_ERROR_WITH_LOCATION() << "Invaild params: " << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }
    auto seq = &d_qos->unicast_locator_seq;

    if (d_qos->localhost_only == DAL_TRUE) {
        return DAL_RETCODE_OK;
    }

    uint32_t unicast_locator_num = SIZE_SEQUENCE(dal_locator_t, seq);
    if (unicast_locator_num == 0)
        return DAL_RETCODE_OK;

    DDS_Long unicast_arr_length = unicast_locator_num;
    DDS_TransportUnicastSettings_t* unicast_array =
        new DDS_TransportUnicastSettings_t[unicast_arr_length];

    dal_locator_t* d_locator = (dal_locator_t*)calloc(sizeof(dal_locator_t), 1);
    for (int i = 0; i < unicast_locator_num; i++) {
        memset(d_locator, 0, sizeof(dal_locator_t));
        GET_ELEMENT_SEQUENCE(dal_locator_t, seq, i, d_locator);
        auto ip_str = std::string((const char*)(d_locator->address),
                                  GetStringLengthInByteArray(d_locator->address));
        auto ips = Split(ip_str, '.');
        if (ips.size() != 4) {
            LOG_ERROR_WITH_LOCATION() << "unicast_locator invalid address: " << ip_str << LOG_ENDL;
            return DAL_RETCODE_BAD_PARAMETER;
        }
        if (ip_str.compare("127.0.0.1") == 0) {
            continue;
        }
        unicast_array->receive_port = d_locator->port;
        const char* ip_arr[4];
        for (int i = 0; i < 4; i++) {
            ip_arr[i] = ips[i].c_str();
        }
        // unicast_array->transports.from_array(ip_arr, 4);
        DDS_StringSeq_from_array(&unicast_array->transports, ip_arr, 4);
    }

    // r_qos->default_unicast.value.from_array(unicast_array, unicast_arr_length);
    DDS_TransportUnicastSettingsSeq_from_array(
        &r_qos->default_unicast.value, unicast_array, unicast_arr_length);

    delete[] unicast_array;
    free(d_locator);
    return DAL_RETCODE_OK;
}

dal_ret_t QosConverter::SetMulticastToRtiddsParticipantQos(dal_domain_participant_qos_t* d_qos,
                                                           DDS_DomainParticipantQos* r_qos)
{
    if (d_qos == nullptr) {
        LOG_ERROR_WITH_LOCATION() << "Invaild params: " << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }
    auto seq = &d_qos->multicast_locator_seq;

    // only allow multicast used in spdp.
    uint32_t multicast_locator_num = SIZE_SEQUENCE(dal_locator_t, seq);
    if (multicast_locator_num == 0) {
        // default multicast 239.255.0.1
        return DAL_RETCODE_OK;
    }

    dal_locator_t* d_locator = (dal_locator_t*)calloc(sizeof(dal_locator_t), 1);
    if (d_locator == nullptr) {
        LOG_ERROR_WITH_LOCATION() << "calloc dal_locator_t failed: " << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    DDS_Long multicast_arr_length = multicast_locator_num;
    DDS_TransportMulticastMapping_t* multicast_array =
        new DDS_TransportMulticastMapping_t[multicast_arr_length];
    char* ipstr_arr[multicast_locator_num];

    for (int i = 0; i < multicast_locator_num; i++) {
        memset(d_locator, 0, sizeof(dal_locator_t));
        GET_ELEMENT_SEQUENCE(dal_locator_t, seq, i, d_locator);
        std::string ip_str = std::string((const char*)(d_locator->address),
                                         GetStringLengthInByteArray(d_locator->address));
        auto ips = Split(ip_str, '.');
        if (ips.size() != 4) {
            LOG_ERROR_WITH_LOCATION()
                << "multicast_locator invalid address: " << ip_str << LOG_ENDL;
            return DAL_RETCODE_BAD_PARAMETER;
        }
        ipstr_arr[i] = new char[ip_str.size() + 1]();
        memcpy(ipstr_arr, ip_str.c_str(), ip_str.size());
        multicast_array[i].addresses = ipstr_arr[i];
    }

    // r_qos->multicast_mapping.value.from_array(multicast_array, multicast_arr_length);
    DDS_TransportMulticastMappingSeq_from_array(
        &r_qos->multicast_mapping.value, multicast_array, multicast_arr_length);

    for (int i = 0; i < multicast_locator_num; i++) {
        delete ipstr_arr[i];
    }

    delete[] multicast_array;
    free(d_locator);
    return DAL_RETCODE_OK;
}

void QosConverter::CreateFlowController(DDS_DomainParticipant* r_qos)
{
    // Configures a FlowController of 838 Mbps (~ 100 MB/sec)
    DDS_FlowControllerProperty_t fc_property_838Mbps = DDS_FLOW_CONTROLLER_PROPERTY_DEFAULT;
    fc_property_838Mbps.scheduling_policy = DDS_RR_FLOW_CONTROLLER_SCHED_POLICY;
    fc_property_838Mbps.token_bucket.max_tokens = 128;
    fc_property_838Mbps.token_bucket.tokens_added_per_period = 128;
    fc_property_838Mbps.token_bucket.bytes_per_token = 8192;
    fc_property_838Mbps.token_bucket.period.sec = 0;
    fc_property_838Mbps.token_bucket.period.nanosec = 10000000;

    DDS_FlowController* fc_838Mbps = DDS_DomainParticipant_create_flowcontroller(
        r_qos, "RTIDDS_FlowController_838Mbps", &fc_property_838Mbps);
    if (!fc_838Mbps) {
        LOG_ERROR_WITH_LOCATION() << "Create flow controller failed" << LOG_ENDL;
    }

    // Configures a FlowController of 628 Mbps (~ 75 MB/sec)
    DDS_FlowControllerProperty_t fc_property_628Mbps = DDS_FLOW_CONTROLLER_PROPERTY_DEFAULT;
    fc_property_628Mbps.scheduling_policy = DDS_RR_FLOW_CONTROLLER_SCHED_POLICY;
    fc_property_628Mbps.token_bucket.max_tokens = 96;
    fc_property_628Mbps.token_bucket.tokens_added_per_period = 96;
    fc_property_628Mbps.token_bucket.bytes_per_token = 8192;
    fc_property_628Mbps.token_bucket.period.sec = 0;
    fc_property_628Mbps.token_bucket.period.nanosec = 10000000;

    DDS_FlowController* fc_628Mbps = DDS_DomainParticipant_create_flowcontroller(
        r_qos, "RTIDDS_FlowController_628Mbps", &fc_property_628Mbps);
    if (!fc_628Mbps) {
        LOG_ERROR_WITH_LOCATION() << "Create flow controller failed" << LOG_ENDL;
    }

    // Configures a FlowController of 420 Mbps (~ 50 MB/sec)
    DDS_FlowControllerProperty_t fc_property_420Mbps = DDS_FLOW_CONTROLLER_PROPERTY_DEFAULT;
    fc_property_420Mbps.scheduling_policy = DDS_RR_FLOW_CONTROLLER_SCHED_POLICY;
    fc_property_420Mbps.token_bucket.max_tokens = 64;
    fc_property_420Mbps.token_bucket.tokens_added_per_period = 64;
    fc_property_420Mbps.token_bucket.bytes_per_token = 8192;
    fc_property_420Mbps.token_bucket.period.sec = 0;
    fc_property_420Mbps.token_bucket.period.nanosec = 10000000;

    DDS_FlowController* fc_420Mbps = DDS_DomainParticipant_create_flowcontroller(
        r_qos, "RTIDDS_FlowController_420Mbps", &fc_property_420Mbps);
    if (!fc_420Mbps) {
        LOG_ERROR_WITH_LOCATION() << "Create flow controller failed" << LOG_ENDL;
    }

    // Configures a FlowController of 209 Mbps (~ 25 MB/sec)
    DDS_FlowControllerProperty_t fc_property_209Mbps = DDS_FLOW_CONTROLLER_PROPERTY_DEFAULT;
    fc_property_209Mbps.scheduling_policy = DDS_RR_FLOW_CONTROLLER_SCHED_POLICY;
    fc_property_209Mbps.token_bucket.max_tokens = 32;
    fc_property_209Mbps.token_bucket.tokens_added_per_period = 32;
    fc_property_209Mbps.token_bucket.bytes_per_token = 8192;
    fc_property_209Mbps.token_bucket.period.sec = 0;
    fc_property_209Mbps.token_bucket.period.nanosec = 10000000;

    DDS_FlowController* fc_209Mbps = DDS_DomainParticipant_create_flowcontroller(
        r_qos, "RTIDDS_FlowController_209Mbps", &fc_property_209Mbps);
    if (!fc_209Mbps) {
        LOG_ERROR_WITH_LOCATION() << "Create flow controller failed" << LOG_ENDL;
    }

    // Configures a FlowController of 100 Mbps (~ 12.5 MB/sec)
    DDS_FlowControllerProperty_t fc_property_100Mbps = DDS_FLOW_CONTROLLER_PROPERTY_DEFAULT;
    fc_property_100Mbps.scheduling_policy = DDS_RR_FLOW_CONTROLLER_SCHED_POLICY;
    fc_property_100Mbps.token_bucket.max_tokens = 16;
    fc_property_100Mbps.token_bucket.tokens_added_per_period = 16;
    fc_property_100Mbps.token_bucket.bytes_per_token = 8192;
    fc_property_100Mbps.token_bucket.period.sec = 0;
    fc_property_100Mbps.token_bucket.period.nanosec = 10000000;

    DDS_FlowController* fc_100Mbps = DDS_DomainParticipant_create_flowcontroller(
        r_qos, "RTIDDS_FlowController_100Mbps", &fc_property_100Mbps);
    if (!fc_100Mbps) {
        LOG_ERROR_WITH_LOCATION() << "Create flow controller failed" << LOG_ENDL;
    }

    // Configures a FlowController of 52 Mbps (~ 6.25 MB/sec)
    DDS_FlowControllerProperty_t fc_property_52Mbps = DDS_FLOW_CONTROLLER_PROPERTY_DEFAULT;
    fc_property_52Mbps.scheduling_policy = DDS_RR_FLOW_CONTROLLER_SCHED_POLICY;
    fc_property_52Mbps.token_bucket.max_tokens = 8;
    fc_property_52Mbps.token_bucket.tokens_added_per_period = 8;
    fc_property_52Mbps.token_bucket.bytes_per_token = 8192;
    fc_property_52Mbps.token_bucket.period.sec = 0;
    fc_property_52Mbps.token_bucket.period.nanosec = 10000000;

    DDS_FlowController* fc_52Mbps = DDS_DomainParticipant_create_flowcontroller(
        r_qos, "RTIDDS_FlowController_52Mbps", &fc_property_52Mbps);
    if (!fc_52Mbps) {
        LOG_ERROR_WITH_LOCATION() << "Create flow controller failed" << LOG_ENDL;
    }
}

void QosConverter::OptimizeParticipantQosInCyberRT(DDS_DomainParticipantQos* r_qos)
{
    // reduce shutdown_cleanup_period in DATABASE QosPolicy in ParticipantQos.
    r_qos->database.shutdown_cleanup_period.sec = 0;
    r_qos->database.shutdown_cleanup_period.nanosec = 10000000;

    // accelarate PDP.
    r_qos->discovery_config.min_initial_participant_announcement_period.sec = 0;
    r_qos->discovery_config.min_initial_participant_announcement_period.nanosec = 1000000;
    r_qos->discovery_config.max_initial_participant_announcement_period.sec = 0;
    r_qos->discovery_config.max_initial_participant_announcement_period.nanosec = 1000000;
    r_qos->discovery_config.initial_participant_announcements = 20;

    // accelarate EDP.
    r_qos->discovery_config.publication_reader.heartbeat_suppression_duration.sec = 0;
    r_qos->discovery_config.publication_reader.heartbeat_suppression_duration.nanosec = 0;
    r_qos->discovery_config.publication_reader.nack_period.sec = 0;
    r_qos->discovery_config.publication_reader.nack_period.nanosec = 100000000;

    r_qos->discovery_config.subscription_reader.heartbeat_suppression_duration.sec = 0;
    r_qos->discovery_config.subscription_reader.heartbeat_suppression_duration.nanosec = 0;
    r_qos->discovery_config.subscription_reader.nack_period.sec = 0;
    r_qos->discovery_config.subscription_reader.nack_period.nanosec = 100000000;

    r_qos->discovery_config.publication_writer.late_joiner_heartbeat_period.sec = 0;
    r_qos->discovery_config.publication_writer.late_joiner_heartbeat_period.nanosec = 100000000;
    r_qos->discovery_config.publication_writer.fast_heartbeat_period.sec = 0;
    r_qos->discovery_config.publication_writer.fast_heartbeat_period.nanosec = 100000000;
    r_qos->discovery_config.publication_writer.max_heartbeat_retries = 300;

    r_qos->discovery_config.subscription_writer.late_joiner_heartbeat_period.sec = 0;
    r_qos->discovery_config.subscription_writer.late_joiner_heartbeat_period.nanosec = 100000000;
    r_qos->discovery_config.subscription_writer.fast_heartbeat_period.sec = 0;
    r_qos->discovery_config.subscription_writer.fast_heartbeat_period.nanosec = 100000000;
    r_qos->discovery_config.subscription_writer.max_heartbeat_retries = 300;

    // set duration of participant liveliness_lease and liveliness_assert.
    r_qos->discovery_config.participant_liveliness_lease_duration.sec = 12;
    r_qos->discovery_config.participant_liveliness_lease_duration.nanosec = 0;
    r_qos->discovery_config.participant_liveliness_assert_period.sec = 3;
    r_qos->discovery_config.participant_liveliness_assert_period.nanosec = 0;

    r_qos->discovery_config.max_liveliness_loss_detection_period.sec = 12;
    r_qos->discovery_config.max_liveliness_loss_detection_period.nanosec = 0;

    // for async transmittion of large size data in UDPv4.
    // MTU=1500, set it less than it so each RTPS message can fit in a single Ethernet Frame.
    // https://community.rti.com/kb/i-am-not-receiving-samples-could-it-be-ip-fragmentation.
    // After testing, we find that set dds.transport.UDPv4.builtin.parent.message_size_max=1475 
    // will case higher cpu usage and higher latency, so set it default.
    // DDS::PropertyQosPolicyHelper::assert_property(r_qos->property,
    //     "dds.transport.UDPv4.builtin.parent.message_size_max", "1475", DDS_BOOLEAN_FALSE);

    // https://community.rti.com/kb/what-error-reliable-large-data-requires-asynchronous-write
    r_qos->receiver_pool.buffer_size = 105000000; //10M
    DDS::PropertyQosPolicyHelper::assert_property(r_qos->property,
        "dds.transport.UDPv4.builtin.recv_socket_buffer_size", "30000000", DDS_BOOLEAN_FALSE);
    DDS::PropertyQosPolicyHelper::assert_property(r_qos->property,
        "dds.transport.UDPv4.builtin.send_socket_buffer_size", "30000000", DDS_BOOLEAN_FALSE);

    // for async transmittion of large size data in SHM.
    // set message_size_max larger than 10M, so large data can be transmit in shm,
    // and receive_buffer_size > message_size_max.
    DDS::PropertyQosPolicyHelper::assert_property(r_qos->property,
        "dds.transport.shmem.builtin.parent.message_size_max", "10500000", DDS_BOOLEAN_FALSE);
    DDS::PropertyQosPolicyHelper::assert_property(r_qos->property,
        "dds.transport.shmem.builtin.receive_buffer_size", "13107200", DDS_BOOLEAN_FALSE);
    DDS::PropertyQosPolicyHelper::assert_property(r_qos->property,
        "dds.transport.shmem.builtin.received_message_count_max", "8", DDS_BOOLEAN_FALSE);

    // set max_size to infinite to dynamic allocate mem(<absolute_max_size).
    // DDS::PropertyQosPolicyHelper::add_property(
    //     r_qos->property, "dds.builtin_type.octets.max_size", "30000000", DDS::BOOLEAN_FALSE);
}

dal_ret_t QosConverter::ConvertParticipantQosFromDalToRtidds(dal_domain_participant_qos_t* d_qos,
                                                             DDS_DomainParticipantQos* r_qos)
{
    if (r_qos == nullptr) {
        LOG_ERROR_WITH_LOCATION() << "Rticonnextdds qos is NULL" << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    if (d_qos == DAL_PARTICIPANT_QOS_DEFAULT) {
        return DAL_RETCODE_OK;
    }

    int seq_size = SIZE_SEQUENCE(octet, &d_qos->user_data.value);
    if (0l != seq_size) {
        DDS_Octet* data_arr = new DDS_Octet[seq_size]();
        uint8_t val;
        for (int i = 0; i < seq_size; i++) {
            val = 0;
            GET_ELEMENT_SEQUENCE(octet, &d_qos->user_data.value, i, &val);
            memcpy(data_arr + i, &val, sizeof(uint8_t));
        }
        // bool ret = r_qos->user_data.value.from_array(data_arr, seq_size);
        DDS_OctetSeq_from_array(&r_qos->user_data.value, data_arr, seq_size);
        delete[] data_arr;
    }

    r_qos->entity_factory.autoenable_created_entities =
        d_qos->entity_factory.autoenable_created_entities;

    if (d_qos->participant_name) {
        r_qos->participant_name.name = (char*)malloc(strlen(d_qos->participant_name) + 1);
        memcpy(
            r_qos->participant_name.name, d_qos->participant_name, strlen(d_qos->participant_name));
        (r_qos->participant_name.name)[strlen(d_qos->participant_name)] = '\0';
    }

    return DAL_RETCODE_OK;
}

dal_ret_t QosConverter::ConvertPublisherQosFromDalToRtidds(dal_publisher_qos_t* d_qos,
                                                           DDS_PublisherQos* r_qos)
{
    if (r_qos == nullptr) {
        LOG_ERROR_WITH_LOCATION() << "Rticonnextdds qos is NULL" << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }
    if (d_qos == DAL_PUBLISHER_QOS_DEFAULT) {
        return DAL_RETCODE_OK;
    }

    switch (d_qos->presentation.access_scope) {
        default:
        case DAL_INSTANCE_PRESENTATION_QOS:
            r_qos->presentation.access_scope = DDS_INSTANCE_PRESENTATION_QOS;
            break;
        case DAL_TOPIC_PRESENTATION_QOS:
            r_qos->presentation.access_scope = DDS_TOPIC_PRESENTATION_QOS;
            break;
        case DAL_GROUP_PRESENTATION_QOS:
            r_qos->presentation.access_scope = DDS_GROUP_PRESENTATION_QOS;
            break;
    }
    r_qos->presentation.coherent_access = d_qos->presentation.coherent_access;
    r_qos->presentation.ordered_access = d_qos->presentation.ordered_access;

    r_qos->entity_factory.autoenable_created_entities =
        d_qos->entity_factory.autoenable_created_entities;

    int seq_size = SIZE_SEQUENCE(octet, &d_qos->group_data.value);
    if (0l != seq_size) {
        DDS_Octet* data_arr = new DDS_Octet[seq_size]();
        uint8_t val;
        for (int i = 0; i < seq_size; i++) {
            val = 0;
            GET_ELEMENT_SEQUENCE(octet, &d_qos->group_data.value, i, &val);
            memcpy(data_arr + i, &val, sizeof(uint8_t));
        }
        // r_qos->group_data.value.from_array(data_arr, seq_size);
        DDS_OctetSeq_from_array(&r_qos->group_data.value, data_arr, seq_size);
        delete[] data_arr;
    }

    if (d_qos->partition.name.n > 0) {
        // r_qos->partition.name.from_array((const char**)d_qos->partition.name.strs,
        //                                  d_qos->partition.name.n);
        DDS_StringSeq_from_array(&r_qos->partition.name,
                                 (const char**)d_qos->partition.name.strs,
                                 d_qos->partition.name.n);
    }

    return DAL_RETCODE_OK;
}

dal_ret_t QosConverter::ConvertSubscriberQosFromDalToRtidds(dal_subscriber_qos_t* d_qos,
                                                            DDS_SubscriberQos* r_qos)
{
    if (r_qos == nullptr) {
        LOG_ERROR_WITH_LOCATION() << "Rticonnextdds qos is NULL" << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }
    if (d_qos == DAL_SUBSCRIBER_QOS_DEFAULT) {
        return DAL_RETCODE_OK;
    }

    switch (d_qos->presentation.access_scope) {
        default:
        case DAL_INSTANCE_PRESENTATION_QOS:
            r_qos->presentation.access_scope = DDS_INSTANCE_PRESENTATION_QOS;
            break;
        case DAL_TOPIC_PRESENTATION_QOS:
            r_qos->presentation.access_scope = DDS_TOPIC_PRESENTATION_QOS;
            break;
        case DAL_GROUP_PRESENTATION_QOS:
            r_qos->presentation.access_scope = DDS_GROUP_PRESENTATION_QOS;
            break;
    }
    r_qos->presentation.coherent_access = d_qos->presentation.coherent_access;
    r_qos->presentation.ordered_access = d_qos->presentation.ordered_access;

    r_qos->entity_factory.autoenable_created_entities =
        d_qos->entity_factory.autoenable_created_entities;

    int seq_size = SIZE_SEQUENCE(octet, &d_qos->group_data.value);
    if (0l != seq_size) {
        DDS_Octet* data_arr = new DDS_Octet[seq_size]();
        uint8_t val;
        for (int i = 0; i < seq_size; i++) {
            val = 0;
            GET_ELEMENT_SEQUENCE(octet, &d_qos->group_data.value, i, &val);
            memcpy(data_arr + i, &val, sizeof(uint8_t));
        }
        // r_qos->group_data.value.from_array(data_arr, seq_size);
        DDS_OctetSeq_from_array(&r_qos->group_data.value, data_arr, seq_size);
        delete[] data_arr;
    }

    if (d_qos->partition.name.n > 0) {
        // r_qos->partition.name.from_array((const char**)d_qos->partition.name.strs,
        //                                  d_qos->partition.name.n);
        DDS_StringSeq_from_array(&r_qos->partition.name,
                                 (const char**)d_qos->partition.name.strs,
                                 d_qos->partition.name.n);
    }

    return DAL_RETCODE_OK;
}

dal_ret_t QosConverter::ConvertTopicQosFromDalToRtidds(dal_topic_qos_t* d_qos, DDS_TopicQos* r_qos)
{
    if (r_qos == nullptr) {
        LOG_ERROR_WITH_LOCATION() << "Rticonnextdds qos is NULL" << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }
    if (d_qos == DAL_TOPIC_QOS_DEFAULT) {
        return DAL_RETCODE_OK;
    }

    int seq_size = SIZE_SEQUENCE(octet, &d_qos->topic_data.value);
    if (0l != seq_size) {
        DDS_Octet* data_arr = new DDS_Octet[seq_size]();
        uint8_t val;
        for (int i = 0; i < seq_size; i++) {
            val = 0;
            GET_ELEMENT_SEQUENCE(octet, &d_qos->topic_data.value, i, &val);
            memcpy(data_arr + i, &val, sizeof(uint8_t));
        }
        // r_qos->topic_data.value.from_array(data_arr, seq_size);
        DDS_OctetSeq_from_array(&r_qos->topic_data.value, data_arr, seq_size);
        delete[] data_arr;
    }

    switch (d_qos->durability.kind) {
        default:
        case DAL_VOLATILE_DURABILITY_QOS:
            r_qos->durability.kind = DDS_VOLATILE_DURABILITY_QOS;
            break;
        case DAL_TRANSIENT_LOCAL_DURABILITY_QOS:
            r_qos->durability.kind = DDS_TRANSIENT_LOCAL_DURABILITY_QOS;
            break;
        case DAL_TRANSIENT_DURABILITY_QOS:
            r_qos->durability.kind = DDS_TRANSIENT_DURABILITY_QOS;
            break;
        case DAL_PERSISTENT_DURABILITY_QOS:
            r_qos->durability.kind = DDS_PERSISTENT_DURABILITY_QOS;
            break;
    }

    r_qos->durability_service.service_cleanup_delay = {
        (int32_t)d_qos->durability_service.service_cleanup_delay.sec,
        (uint32_t)d_qos->durability_service.service_cleanup_delay.nanosec};

    switch (d_qos->durability_service.history_kind) {
        default:
        case DAL_KEEP_LAST_HISTORY_QOS:
            r_qos->durability_service.history_kind = DDS_KEEP_LAST_HISTORY_QOS;
            break;
        case DAL_KEEP_ALL_HISTORY_QOS:
            r_qos->durability_service.history_kind = DDS_KEEP_ALL_HISTORY_QOS;
            break;
    }
    r_qos->durability_service.history_depth = d_qos->durability_service.history_depth;
    r_qos->durability_service.max_samples = d_qos->durability_service.max_samples;
    r_qos->durability_service.max_instances = d_qos->durability_service.max_instances;
    r_qos->durability_service.max_samples_per_instance =
        d_qos->durability_service.max_samples_per_instance;

    r_qos->deadline.period = {(int32_t)d_qos->deadline.period.sec,
                              (uint32_t)d_qos->deadline.period.nanosec};

    r_qos->latency_budget.duration = {(int32_t)d_qos->latency_budget.duration.sec,
                                      (uint32_t)d_qos->latency_budget.duration.nanosec};

    switch (d_qos->liveliness.kind) {
        default:
        case DAL_AUTOMATIC_LIVELINESS_QOS:
            r_qos->liveliness.kind = DDS_AUTOMATIC_LIVELINESS_QOS;
            break;
        case DAL_MANUAL_BY_PARTICIPANT_LIVELINESS_QOS:
            r_qos->liveliness.kind = DDS_MANUAL_BY_PARTICIPANT_LIVELINESS_QOS;
            break;
        case DAL_MANUAL_BY_TOPIC_LIVELINESS_QOS:
            r_qos->liveliness.kind = DDS_MANUAL_BY_TOPIC_LIVELINESS_QOS;
            break;
    }

    r_qos->liveliness.lease_duration = {(int32_t)d_qos->liveliness.lease_duration.sec,
                                        (uint32_t)d_qos->liveliness.lease_duration.nanosec};

    switch (d_qos->reliability.kind) {
        default:
        case DAL_BEST_EFFORT_RELIABILITY_QOS:
            r_qos->reliability.kind = DDS_BEST_EFFORT_RELIABILITY_QOS;
            break;
        case DAL_RELIABLE_RELIABILITY_QOS:
            r_qos->reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
            break;
    }
    r_qos->reliability.max_blocking_time = {(int32_t)d_qos->reliability.max_blocking_time.sec,
                                            (uint32_t)d_qos->reliability.max_blocking_time.nanosec};

    switch (d_qos->destination_order.kind) {
        default:
        case DAL_BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS:
            r_qos->destination_order.kind = DDS_BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS;
            break;
        case DAL_BY_SOURCE_TIMESTAMP_DESTINATIONORDER_QOS:
            r_qos->destination_order.kind = DDS_BY_SOURCE_TIMESTAMP_DESTINATIONORDER_QOS;
            break;
    }

    switch (d_qos->history.kind) {
        default:
        case DAL_KEEP_LAST_HISTORY_QOS:
            r_qos->history.kind = DDS_KEEP_LAST_HISTORY_QOS;
            break;
        case DAL_KEEP_ALL_HISTORY_QOS:
            r_qos->history.kind = DDS_KEEP_ALL_HISTORY_QOS;
            break;
    }
    r_qos->history.depth = d_qos->history.depth;

    r_qos->resource_limits.max_samples = d_qos->resource_limits.max_samples;
    r_qos->resource_limits.initial_samples = 1;
    r_qos->resource_limits.max_instances = d_qos->resource_limits.max_instances;
    r_qos->resource_limits.initial_instances = 1;
    r_qos->resource_limits.max_samples_per_instance =
        d_qos->resource_limits.max_samples_per_instance;

    r_qos->transport_priority.value = d_qos->transport_priority.value;

    r_qos->lifespan.duration = {(int32_t)d_qos->lifespan.duration.sec,
                                (uint32_t)d_qos->lifespan.duration.nanosec};

    switch (d_qos->ownership.kind) {
        default:
        case DAL_SHARED_OWNERSHIP_QOS:
            r_qos->ownership.kind = DDS_SHARED_OWNERSHIP_QOS;
            break;
        case DAL_EXCLUSIVE_OWNERSHIP_QOS:
            r_qos->ownership.kind = DDS_EXCLUSIVE_OWNERSHIP_QOS;
            break;
    }

    return DAL_RETCODE_OK;
}

dal_ret_t QosConverter::ConvertDataWriterQosFromDalToRtidds(dal_datawriter_qos_t* d_qos,
                                                            DDS_DataWriterQos* r_qos)
{
    if (r_qos == nullptr) {
        LOG_ERROR_WITH_LOCATION() << "Rticonnextdds qos is NULL" << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    if (d_qos == DAL_DATAWRITER_QOS_DEFAULT) {
        return DAL_RETCODE_OK;
    }

    switch (d_qos->durability.kind) {
        default:
        case DAL_VOLATILE_DURABILITY_QOS:
            r_qos->durability.kind = DDS_VOLATILE_DURABILITY_QOS;
            break;
        case DAL_TRANSIENT_LOCAL_DURABILITY_QOS:
            r_qos->durability.kind = DDS_TRANSIENT_LOCAL_DURABILITY_QOS;
            break;
        case DAL_TRANSIENT_DURABILITY_QOS:
            r_qos->durability.kind = DDS_TRANSIENT_DURABILITY_QOS;
            break;
        case DAL_PERSISTENT_DURABILITY_QOS:
            r_qos->durability.kind = DDS_PERSISTENT_DURABILITY_QOS;
            break;
    }

    r_qos->durability_service.service_cleanup_delay = {
        (int32_t)d_qos->durability_service.service_cleanup_delay.sec,
        (uint32_t)d_qos->durability_service.service_cleanup_delay.nanosec};
    switch (d_qos->durability_service.history_kind) {
        default:
        case DAL_KEEP_LAST_HISTORY_QOS:
            r_qos->durability_service.history_kind = DDS_KEEP_LAST_HISTORY_QOS;
            break;
        case DAL_KEEP_ALL_HISTORY_QOS:
            r_qos->durability_service.history_kind = DDS_KEEP_ALL_HISTORY_QOS;
            break;
    }

    r_qos->durability_service.history_depth = d_qos->durability_service.history_depth;
    r_qos->durability_service.max_samples = d_qos->durability_service.max_samples;
    r_qos->durability_service.max_instances = d_qos->durability_service.max_instances;
    r_qos->durability_service.max_samples_per_instance =
        d_qos->durability_service.max_samples_per_instance;

    r_qos->deadline.period = {(int32_t)d_qos->deadline.period.sec,
                              (uint32_t)d_qos->deadline.period.nanosec};

    r_qos->latency_budget.duration = {(int32_t)d_qos->latency_budget.duration.sec,
                                      (uint32_t)d_qos->latency_budget.duration.nanosec};

    switch (d_qos->liveliness.kind) {
        default:
        case DAL_AUTOMATIC_LIVELINESS_QOS:
            r_qos->liveliness.kind = DDS_AUTOMATIC_LIVELINESS_QOS;
            break;
        case DAL_MANUAL_BY_PARTICIPANT_LIVELINESS_QOS:
            r_qos->liveliness.kind = DDS_MANUAL_BY_PARTICIPANT_LIVELINESS_QOS;
            break;
        case DAL_MANUAL_BY_TOPIC_LIVELINESS_QOS:
            r_qos->liveliness.kind = DDS_MANUAL_BY_TOPIC_LIVELINESS_QOS;
            break;
    }

    r_qos->liveliness.lease_duration = {(int32_t)d_qos->liveliness.lease_duration.sec,
                                        (uint32_t)d_qos->liveliness.lease_duration.nanosec};

    switch (d_qos->reliability.kind) {
        default:
        case DAL_RELIABLE_RELIABILITY_QOS:
            r_qos->reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
            break;
        case DAL_BEST_EFFORT_RELIABILITY_QOS:
            r_qos->reliability.kind = DDS_BEST_EFFORT_RELIABILITY_QOS;
            break;
    }
    r_qos->reliability.max_blocking_time = {(int32_t)d_qos->reliability.max_blocking_time.sec,
                                            (uint32_t)d_qos->reliability.max_blocking_time.nanosec};

    switch (d_qos->destination_order.kind) {
        default:
        case DAL_BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS:
            r_qos->destination_order.kind = DDS_BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS;
            break;
        case DAL_BY_SOURCE_TIMESTAMP_DESTINATIONORDER_QOS:
            r_qos->destination_order.kind = DDS_BY_SOURCE_TIMESTAMP_DESTINATIONORDER_QOS;
            break;
    }

    switch (d_qos->history.kind) {
        default:
        case DAL_KEEP_LAST_HISTORY_QOS:
            r_qos->history.kind = DDS_KEEP_LAST_HISTORY_QOS;
            break;
        case DAL_KEEP_ALL_HISTORY_QOS:
            r_qos->history.kind = DDS_KEEP_ALL_HISTORY_QOS;
            break;
    }
    r_qos->history.depth = d_qos->history.depth;

    r_qos->resource_limits.max_samples = d_qos->resource_limits.max_samples;
    r_qos->resource_limits.initial_samples = 1;
    r_qos->resource_limits.max_instances = d_qos->resource_limits.max_instances;
    r_qos->resource_limits.initial_instances = 1;
    r_qos->resource_limits.max_samples_per_instance =
        d_qos->resource_limits.max_samples_per_instance;

    r_qos->transport_priority.value = d_qos->transport_priority.value;

    r_qos->lifespan.duration = {(int32_t)d_qos->lifespan.duration.sec,
                                (uint32_t)d_qos->lifespan.duration.nanosec};

    int seq_size = SIZE_SEQUENCE(octet, &d_qos->user_data.value);
    if (seq_size > 0) {
        DDS_Octet* data_arr = new DDS_Octet[seq_size]();
        uint8_t val;
        for (int i = 0; i < seq_size; i++) {
            val = 0;
            GET_ELEMENT_SEQUENCE(octet, &d_qos->user_data.value, i, &val);
            memcpy(data_arr + i, &val, sizeof(uint8_t));
        }
        // r_qos->user_data.value.from_array(data_arr, seq_size);
        DDS_OctetSeq_from_array(&r_qos->user_data.value, data_arr, seq_size);
        delete[] data_arr;
    }

    switch (d_qos->ownership.kind) {
        default:
        case DAL_SHARED_OWNERSHIP_QOS:
            r_qos->ownership.kind = DDS_SHARED_OWNERSHIP_QOS;
            break;
        case DAL_EXCLUSIVE_OWNERSHIP_QOS:
            r_qos->ownership.kind = DDS_EXCLUSIVE_OWNERSHIP_QOS;
            break;
    }

    r_qos->ownership_strength.value = d_qos->ownership_strength.value;

    r_qos->writer_data_lifecycle.autodispose_unregistered_instances =
        d_qos->writer_data_lifecycle.autodispose_unregistered_instances;

    return DAL_RETCODE_OK;
}

dal_ret_t QosConverter::ConvertDataReaderQosFromDalToRtidds(dal_datareader_qos_t* d_qos,
                                                            DDS_DataReaderQos* r_qos)
{
    if (r_qos == nullptr) {
        LOG_ERROR_WITH_LOCATION() << "Rticonnextdds qos is NULL" << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    if (d_qos == DAL_DATAREADER_QOS_DEFAULT) {
        return DAL_RETCODE_OK;
    }

    switch (d_qos->durability.kind) {
        default:
        case DAL_VOLATILE_DURABILITY_QOS:
            r_qos->durability.kind = DDS_VOLATILE_DURABILITY_QOS;
            break;
        case DAL_TRANSIENT_LOCAL_DURABILITY_QOS:
            r_qos->durability.kind = DDS_TRANSIENT_LOCAL_DURABILITY_QOS;
            break;
        case DAL_TRANSIENT_DURABILITY_QOS:
            r_qos->durability.kind = DDS_TRANSIENT_DURABILITY_QOS;
            break;
        case DAL_PERSISTENT_DURABILITY_QOS:
            r_qos->durability.kind = DDS_PERSISTENT_DURABILITY_QOS;
            break;
    }

    r_qos->deadline.period = {(int32_t)d_qos->deadline.period.sec,
                              (uint32_t)d_qos->deadline.period.nanosec};

    r_qos->latency_budget.duration = {(int32_t)d_qos->latency_budget.duration.sec,
                                      (uint32_t)d_qos->latency_budget.duration.nanosec};

    switch (d_qos->liveliness.kind) {
        default:
        case DAL_AUTOMATIC_LIVELINESS_QOS:
            r_qos->liveliness.kind = DDS_AUTOMATIC_LIVELINESS_QOS;
            break;
        case DAL_MANUAL_BY_PARTICIPANT_LIVELINESS_QOS:
            r_qos->liveliness.kind = DDS_MANUAL_BY_PARTICIPANT_LIVELINESS_QOS;
            break;
        case DAL_MANUAL_BY_TOPIC_LIVELINESS_QOS:
            r_qos->liveliness.kind = DDS_MANUAL_BY_TOPIC_LIVELINESS_QOS;
            break;
    }
    r_qos->liveliness.lease_duration = {(int32_t)d_qos->liveliness.lease_duration.sec,
                                        (uint32_t)d_qos->liveliness.lease_duration.nanosec};

    switch (d_qos->reliability.kind) {
        default:
        case DAL_RELIABLE_RELIABILITY_QOS:
            r_qos->reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
            break;
        case DAL_BEST_EFFORT_RELIABILITY_QOS:
            r_qos->reliability.kind = DDS_BEST_EFFORT_RELIABILITY_QOS;
            break;
    }
    r_qos->reliability.max_blocking_time = {(int32_t)d_qos->reliability.max_blocking_time.sec,
                                            (uint32_t)d_qos->reliability.max_blocking_time.nanosec};

    switch (d_qos->destination_order.kind) {
        default:
        case DAL_BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS:
            r_qos->destination_order.kind = DDS_BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS;
            break;
        case DAL_BY_SOURCE_TIMESTAMP_DESTINATIONORDER_QOS:
            r_qos->destination_order.kind = DDS_BY_SOURCE_TIMESTAMP_DESTINATIONORDER_QOS;
            break;
    }

    switch (d_qos->history.kind) {
        default:
        case DAL_KEEP_LAST_HISTORY_QOS:
            r_qos->history.kind = DDS_KEEP_LAST_HISTORY_QOS;
            break;
        case DAL_KEEP_ALL_HISTORY_QOS:
            r_qos->history.kind = DDS_KEEP_ALL_HISTORY_QOS;
            break;
    }
    r_qos->history.depth = d_qos->history.depth;

    r_qos->resource_limits.max_samples = d_qos->resource_limits.max_samples;
    r_qos->resource_limits.initial_samples = 1;
    r_qos->resource_limits.max_instances = d_qos->resource_limits.max_instances;
    r_qos->resource_limits.initial_instances = 1;
    r_qos->resource_limits.max_samples_per_instance =
        d_qos->resource_limits.max_samples_per_instance;

    int seq_size = SIZE_SEQUENCE(octet, &d_qos->user_data.value);
    if (seq_size > 0) {
        DDS_Octet* data_arr = new DDS_Octet[seq_size]();
        uint8_t val;
        for (int i = 0; i < seq_size; i++) {
            val = 0;
            GET_ELEMENT_SEQUENCE(octet, &d_qos->user_data.value, i, &val);
            memcpy(data_arr + i, &val, sizeof(uint8_t));
        }
        // r_qos->user_data.value.from_array(data_arr, seq_size);
        DDS_OctetSeq_from_array(&r_qos->user_data.value, data_arr, seq_size);
        delete[] data_arr;
    }

    switch (d_qos->ownership.kind) {
        default:
        case DAL_SHARED_OWNERSHIP_QOS:
            r_qos->ownership.kind = DDS_SHARED_OWNERSHIP_QOS;
            break;
        case DAL_EXCLUSIVE_OWNERSHIP_QOS:
            r_qos->ownership.kind = DDS_EXCLUSIVE_OWNERSHIP_QOS;
            break;
    }

    r_qos->time_based_filter.minimum_separation = {
        (int32_t)d_qos->time_based_filter.minimum_separation.sec,
        (uint32_t)d_qos->time_based_filter.minimum_separation.nanosec};

    r_qos->reader_data_lifecycle.autopurge_nowriter_samples_delay = {
        (int32_t)d_qos->reader_data_lifecycle.autopurge_nowriter_samples_delay.sec,
        (uint32_t)d_qos->reader_data_lifecycle.autopurge_nowriter_samples_delay.nanosec};

    r_qos->reader_data_lifecycle.autopurge_disposed_samples_delay = {
        (int32_t)d_qos->reader_data_lifecycle.autopurge_disposed_samples_delay.sec,
        (uint32_t)d_qos->reader_data_lifecycle.autopurge_disposed_samples_delay.nanosec};

    return DAL_RETCODE_OK;
}
